#include <server/gdbRDP.h>

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>

#define __GLOG(c, f, ...) \
{ \
    if (lout) \
    { \
        lout(c, f, ##__VA_ARGS__); \
    } \
}

#define GLOG(expr, ...) __GLOG(lctx, expr, ##__VA_ARGS__)

static bool Zz_args_get(gdbPacket &pkt, int &type, uint64_t &addr, int &kind)
{
    char *separator;

    type = strtoul(pkt.data(), &separator, 16);
	if (*separator != ',') {
		LOG_ERROR("incomplete breakpoint/watchpoint packet received");
		return false;
	}
    addr = strtoull(separator + 1, &separator, 16);
	if (*separator != ',') {
		LOG_ERROR("incomplete breakpoint/watchpoint packet received");
		return false;
	}
    kind = strtoul(separator + 1, &separator, 16);

    return true;
}

static int gdb_breakpoint_watchpoint_packet(targetIO &tio, gdbPacket &pkt, bool isAdd)
{
    int type;
    uint64_t addr;
    int kind;
    int ret = -1;

	enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
	enum watchpoint_rw wp_type = WPT_READ /* dummy init to avoid warning */;

    if (Zz_args_get(pkt, type, addr, kind))
    {
        return -1;
    }

	if (type == 0)	/* memory breakpoint */
		bp_type = BKPT_SOFT;
	else if (type == 1)	/* hardware breakpoint */
		bp_type = BKPT_HARD;
	else if (type == 2)	/* write watchpoint */
		wp_type = WPT_WRITE;
	else if (type == 3)	/* read watchpoint */
		wp_type = WPT_READ;
	else if (type == 4)	/* access watchpoint */
		wp_type = WPT_ACCESS;
	else {
		LOG_ERROR("invalid gdb watch/breakpoint type(%d)", type);
		return -1;
	}

	switch (type) {
		case 0:
		case 1:
        {
            if (isAdd)
                ret = tio.insertBreakpoint(addr, kind, bp_type);
            else
                ret = tio.removeBreakpoint(addr);
        }break;
		case 2:
		case 3:
		case 4:
		{

			
		}break;
		default:
        {

        }break;	
	}

    return ret;
}

gdbRDP::gdbRDP()
{
    lctx = 0;
    lout = 0;
    ctrl_c = false;
}

gdbRDP::~gdbRDP()
{

}

void gdbRDP::setLogger(void *ctx, void (*out)(void *, const char *, ...))
{
    lctx = ctx;
    lout = out;
}

void gdbRDP::sendAck(gdbIORemote &rio, char c)
{
    rio.write(&c, 1, 20);
}

int gdbRDP::loop(gdbIORemote &rio, targetIO &tio, gdbPacket &pkt)
{
    int ret;

    ret = readPacket(rio, pkt);
    if (ret < 0)
    {
        goto _out;
    }

    if (ret == 0)
    {
        if (tio.state() == TARGET_RUNNING)
        {
            pollTargetStatus(rio, tio, pkt);
        }

        goto _out;
    }

    ret = command(tio, pkt);
    if (!this->ctrl_c)
        sendAck(rio, '+');
    else
        this->ctrl_c = false;

    if (ret != 0)
    {
        ret = writePacket(rio, pkt);
    }

_out:
    return ret;
}

void gdbRDP::pollTargetStatus(gdbIORemote &rio, targetIO &tio, gdbPacket &pkt)
{
    tio.pollCurStatus();
    if (tio.state() == TARGET_HALTED)
    {
        pkt.resetSendBuf();
        pkt.replyStop();
        writePacket(rio, pkt);
    }
}

int gdbRDP::readPacket(gdbIORemote &rio, gdbPacket &pkt)
{
    int ret = 0;
    char buf[2];

    pkt.resetRecvBuf();
    ret = rio.read(buf, 1, 1000);
    if (ret != 1)
    {
        goto _out;
    }

    if (buf[0] == '+')
    {
        ret = 0;
        goto _out;
    }
    if (buf[0] == '-')
    {
        GLOG("Recv '-'");
        ret = 0;
        goto _out;
    }
    if (buf[0] == 0x03)
    {
        ret = 1;
        pkt.putRecvBuf(buf[0]);
        this->ctrl_c = true;
        goto _out;
    }

    if (buf[0] != '$')
    {
        GLOG("ES(%c, 0x%02x)\n", buf[0], buf[0]);
        ret = 0;
        goto _out;
    }

    while (1)
    {
        ret = rio.read(buf, 1, 10);
        if (ret <= 0)
        {
            goto _out;
        }

        if (!pkt.putRecvBuf(buf[0]))
        {
            GLOG("ERROR: REcvBuf Full!");
            ret = 0;
            goto _out;
        }

        if (buf[0] == '#')
        {
            ret = rio.readAll(buf, 2, 10);
            if (ret <= 0)
                goto _out;
            if (ret != 2)
            {
                ret = 0;
                goto _out;
            }

            pkt.putRecvBuf(buf[0]);
            pkt.putRecvBuf(buf[1]);
            ret = pkt.length();
            break;
        }
    }

    pkt.request()[pkt.length()] = 0;

    GLOG("gdb: %s", pkt.request());

    if (!pkt.check(pkt.length()))
    {
        GLOG("checksum error\n");
        sendAck(rio, '-');
        ret = 0;
        goto _out;
    }

_out:
    return ret;
}

int gdbRDP::writePacket(gdbIORemote &rio, gdbPacket &pkt)
{
    int ret = 0;
    uint32_t len;
    char *sbuf = pkt.response(&len);

    if (rio.nextReadReady())
    {
        GLOG("WARN: next packet ready before repaly");
        return 0;
    }

    if (len)
    {
        ret = rio.write(sbuf, len, 20);
    }

    return ret;
}

int gdbRDP::command(targetIO &tio, gdbPacket &pkt)
{
    int ret = 0;
    int cmd = pkt.getCmd();

    pkt.resetSendBuf();

    switch (cmd)
    {
    case 0x03:
    {
        GLOG("Ctrl-C");
        ret = cmd_CTRL_C(tio, pkt);
    }break;
    case 'q':
    {
        ret = cmd_q(pkt);
    }break;
    case 'v':
    {
        ret = cmd_v(tio, pkt);
    }break;
    case 'H':
    {
        ret = cmd_H(pkt);
    }break;
    case '?':
    {
        ret = cmd_QMark(tio, pkt);
    }break;
    case 'g':
    {
        ret = cmd_g(tio, pkt);
    }break;
    case 'p':
    {
        ret = cmd_p(tio, pkt);
    }break;
    case 'P':
    {
        ret = cmd_P(tio, pkt);
    }break;
    case 'm':
    {
        ret = cmd_m(tio, pkt);
    }break;
    case 'z':
    {
        ret = cmd_z(tio, pkt);
    }break;
    case 'Z':
    {
        ret = cmd_Z(tio, pkt);
    }break;
    case 'c':
    {
        ret = cmd_c(tio, pkt);
    }break;
    case 'D':
    {
        ret = cmd_D(tio, pkt);
    }break;
    case 'T':
    {
        ret = cmd_T(tio, pkt);
    }break;
    case 's':
    {
        ret = cmd_s(tio, pkt);
    }break;
    default:
    {
        GLOG("EC(%c,0x%02x)", cmd, cmd);
    }break;
    }

    return ret;
}

bool gdbRDP::cmd_QMark(targetIO &tio, gdbPacket &pkt)
{
    uint8_t sig = 0;

    //TODO
    tio.pollCurStatus();
    if (!tio.isRunning())
        sig = 2;

    return pkt.replySignal(sig);
}

bool gdbRDP::cmd_s(targetIO &tio, gdbPacket &pkt)
{
	int current = 0;
	uint64_t address = 0x0;
    int retval;

	if (pkt.length() > 1)
		address = strtoull(pkt.data(), NULL, 16);
	else
		current = 1;

    retval = tio.step(current, address, 0);
    //TODO
    pkt.append("T05");
    pkt.makeResponse();

    return true;
}

bool gdbRDP::cmd_T(targetIO &tio, gdbPacket &pkt)
{
    pkt.append("OK");
    pkt.makeResponse();

    return true;
}

bool gdbRDP::cmd_D(targetIO &tio, gdbPacket &pkt)
{
    pkt.append("OK");
    pkt.makeResponse();

    return true;
}

bool gdbRDP::cmd_p(targetIO &tio, gdbPacket &pkt)
{
    char *buf = pkt.data();
    uint32_t regno;
    uint32_t val = 0;
    struct reg r;

    regno = strtoul(buf, NULL, 16);
    r.id = regno;

    tio.readReg(&r);

    pkt.append(r.value, 8); //TODO
    pkt.makeResponse();

    return true;
}

bool gdbRDP::cmd_P(targetIO &tio, gdbPacket &pkt)
{
    char *buf = pkt.data();
    uint32_t regno;
    char *separator;
    struct reg r;

    regno = strtoul(buf, &separator, 16);
    if (*separator != '=') {
        return false;
    }

    r.val64 = strtoull(separator + 1, nullptr, 16);
    r.id = regno;

    tio.writeReg(&r);

    //TODO
    pkt.replyOk();

    return true;
}

bool gdbRDP::cmd_g(targetIO &tio, gdbPacket &pkt)
{
    int regsize = 8;//TODO

    for (int i = 0; i <= 32; i ++)
    {
        struct reg r = {0};

        r.id = i;
        //if (i==32)//TODO
        tio.readReg(&r);
        pkt.append(r.value, regsize);
    }

    pkt.makeResponse();

    return true;
}

int gdbRDP::cmd_q(gdbPacket &pkt)
{
    int ret = 0;

    if (strncmp(pkt.request(), "qSupported", 10) == 0)
    {
        ret = qSupported(pkt);
        goto _out;
    }

    if (strncmp(pkt.request(), "qTStatus", 8) == 0)
    {
        ret = qTStatus(pkt);
        goto _out;
    }

    if (strncmp(pkt.request(), "qXfer", 5) == 0)
    {
        ret = qXfer(pkt);
        goto _out;
    }

    if (strncmp(pkt.request(), "qfThreadInfo", 12) == 0)
    {
        ret = qfThreadInfo(pkt);
        goto _out;
    }

    if (strncmp(pkt.request(), "qsThreadInfo", 12) == 0)
    {
        ret = qsThreadInfo(pkt);
        goto _out;
    }

    if (strncmp(pkt.request(), "qC", 2) == 0)
    {
        ret = qC(pkt);
        goto _out;
    }

    if (strncmp(pkt.request(), "qAttached", 9) == 0)
    {
        ret = qAttached(pkt);
        goto _out;
    }
    
    if (strncmp(pkt.request(), "qOffsets", 8) == 0)
    {
        ret = qOffsets(pkt);
        goto _out;
    }

    if (strncmp(pkt.request(), "qSymbol", 7) == 0)
    {
        ret = qSymbol(pkt);
        goto _out;
    }

    {
        ret = pkt.replyEmpty();
    }

_out:
    return ret;
}

int gdbRDP::cmd_v(targetIO &tio, gdbPacket &pkt)
{
    bool ret = false;
#if 0
    if (strncmp(pkt.request(), "vCont", 5) == 0)
    {
        ret = vCont(tio, pkt);
    }
    else
    {
        ret = pkt.makeResponse();
    }
#else
    ret = pkt.replyEmpty();
#endif
    return ret;
}

bool gdbRDP::cmd_H(gdbPacket &pkt)
{
    return pkt.replyOk();
}

bool gdbRDP::cmd_m(targetIO &tio, gdbPacket &pkt)
{
    uint64_t addr = 0;
    uint32_t len = 0;
    char *separator;
    uint8_t *buffer;
    int retval;

    addr = strtoull(pkt.data(), &separator, 16);

    if (*separator != ',') {
        LOG_ERROR("incomplete read memory packet received, dropping connection");
        return false;
    }

    len = strtoul(separator + 1, NULL, 16);

    if (!len) {
        LOG_WARNING("invalid read memory packet received (len == 0)");
        pkt.append("");
        goto out;
    }

    buffer = (uint8_t*)malloc(len);
    retval = target_read_buffer(tio.getTarget(), addr, len, buffer);
    if (retval != 0)
    {
        memset(buffer, 0, len);
    }

    pkt.append(buffer, len);
    free(buffer);

out:
    pkt.makeResponse();

    return true;
}

bool gdbRDP::cmd_z(targetIO &tio, gdbPacket &pkt) 
{
    int cret;
    bool ret;

    /* remove */
    cret = gdb_breakpoint_watchpoint_packet(tio, pkt, false);
    if (cret == 0)
        ret = pkt.replyOk();
    else
        ret = pkt.replyError();

    return ret;
}

bool gdbRDP::cmd_Z(targetIO &tio, gdbPacket &pkt)
{
    int cret;
    bool ret;

    /* insert */
    cret = gdb_breakpoint_watchpoint_packet(tio, pkt, true);
    if (cret == 0)
        ret = pkt.replyOk();
    else
        ret = pkt.replyError();

    return ret;
}

bool gdbRDP::cmd_c(targetIO &tio, gdbPacket &pkt)
{
    LOG_WARNING("TODO c\n");

    tio.resume(1, 0, 0, 0);
    //pkt.append("OK");
    //pkt.makeResponse();

    return false;
}

bool gdbRDP::cmd_CTRL_C(targetIO &tio, gdbPacket &pkt)
{
    if (tio.isRunning())
    {
        tio.halt();
        tio.pollCurStatus();
    }

    return pkt.replyStop();
}

/************************************/
bool gdbRDP::vCont(targetIO &tio, gdbPacket &pkt)
{
    return pkt.replyEmpty();
}

int gdbRDP::qSupported(gdbPacket &pkt)
{
    pkt.append("PacketSize=");
    pkt.append("200");
    pkt.append(";");

    pkt.append("swbreak+");

    return pkt.makeResponse();
}

bool gdbRDP::qTStatus(gdbPacket &pkt)
{


    return pkt.makeResponse();
}

bool gdbRDP::qXfer(gdbPacket &pkt)
{
    pkt.makeResponse();

    return false;
}

bool gdbRDP::qfThreadInfo(gdbPacket &pkt)
{
    //pkt.append("l");
    pkt.makeResponse();

    return true;
}

bool gdbRDP::qsThreadInfo(gdbPacket &pkt)
{
    pkt.append("l");
    pkt.makeResponse();

    return true;
}

bool gdbRDP::qC(gdbPacket &pkt)
{
    pkt.append("QC1");
    pkt.makeResponse();

    return true;
}

bool gdbRDP::qAttached(gdbPacket &pkt)
{
    pkt.append("1");
    pkt.makeResponse();

    return true;
}

bool gdbRDP::qOffsets(gdbPacket &pkt)
{
    char offsets[] = "Text=0;Data=0;Bss=0";

    pkt.append(offsets, sizeof(offsets)-1);
    pkt.makeResponse();

    return true;
}

bool gdbRDP::qSymbol(gdbPacket &pkt)
{
    pkt.append("OK");
    pkt.makeResponse();

    return true;
}
